#include "common.h"

unsigned int buf[32];
#define NUM 20

#define NUM_NEED 4
unsigned int saved_st[NUM_NEED];

int print_data(void *p)
{
    int i;
    memcpy(buf, p, sizeof(int) * NUM);
    printk("p = %p\n", p);
    
    for (i = 0; i < NUM; ++i) {
        printk("%d = %x\n", i, buf[i]);
    }
    return (0);
}

int func(void)
{
    int i;

    printk(__FUNCTION__);
    printk("\n");
    
    i = 0x104;

    printk("i = %d\n", i);
    
    return (0);
}

int test(int i);
void clean_cache(void *p1, void *p2);
void clean_all_dcache(void);

int func2(void)
{
    void *pfunc;

    printk("tangpl in func2\n");

    clean_cache((void *)func, (void *)func + NUM_NEED);
    printk("tangpl clean cache success\n");

    pfunc = (void *)func;
    memcpy(pfunc, saved_st, 4 * NUM_NEED);

    printk("tangpl copy saved_st success\n");    
    func();

    test(1);
    
    printk(__FUNCTION__);
    printk("\n");
    return (0);    
}

void jmpfunc(void)
{
    void *pfunc2;

    pfunc2 = (void *)func2;
    __asm__ __volatile__ ("mov %%pc, %0"
                          ::"r"(pfunc2));    
}

int test(int i)
{
    void *pfunc, *pjmpfunc;

    pfunc = (void *)func;
    pjmpfunc = (void *)jmpfunc;

    clean_cache((void *)func, (void *)func + NUM_NEED);
    printk("clean cache finish\n");
    
    memcpy(pfunc, pjmpfunc + 4 * 3, 4 * NUM_NEED);

    return (0);
}

int save_old(void)
{
    void *pfunc;
    pfunc = (void *)func;
    memcpy(saved_st, pfunc, 4 * NUM_NEED);
    return (0);
}

/*
		  __asm__ (
		"	mrc p15, 0, %0, c1, c0\n"
		"	mrc p15, 0, %1, c2, c0\n"
		"	mrc p15, 0, %2, c3, c0\n"
		: "=r" (ctrl), "=r" (transbase), "=r" (dac));
*/

void clean_all_dcache(void)
{
    unsigned int sec, index;
    unsigned int i;
    int num = 0;
    
    printk(__FUNCTION__);
    printk("\n");

    for (sec = 0; sec < 8; ++sec) {
        for (index = 0; index < 64; ++index) {
            i = (sec << 5) + (index << 26);
            __asm__ __volatile__ (
                "mcr p15, 0, %0, c7, c14, 2"        
                ::"r"(i));                    
            printk("%d(%d)(%d) = %x\n", num++, sec, index, i);
        }
    }
    return;

    for (i = 0; i < 64; ++i) {
        __asm__ __volatile__ (
            "mcr p15, 0, %0, c7, c14, 2"        
            ::"r"(i << 5));        
    }
}

void clean_cache(void *p1, void *p2)
{
    unsigned int n;

    n = 0;
    clean_all_dcache();
/*
    __asm__ __volatile__ (
        "mcr p15, 0, %0, c7, c14, 1\n"        
        ::"r"(p1));
    __asm__ __volatile__ (
        "mcr p15, 0, %0, c7, c14, 1\n"        
        ::"r"(p2));    
*/
    __asm__ __volatile__ (
        "mcr p15, 0, %0, c7, c5, 0\n"
        ::"r"(0));
    
    
/*    
    for (;;) {
        printk("n = %x\n", n);
        __asm__ __volatile__ (
            "mcr p15, 0, %0, c7, c14, 2\n"        
            :"+r"(n):);
        if (n > (1 << (3 + 5 + 2)))
            break;
        n += (1 << 5);
    }
    __asm__ __volatile__ (
        "mcr p15, 0, %0, c7, c5, 0\n"
        ::"r"(0));
*/    
}

int hijack_func_init(void)
{
    
/*    int i;  */
    void *p;
    printk(__FUNCTION__);
    printk("\n");
    
    save_old();
    printk("save old finish\n");

    test(1);

    printk("test finish\n");

    func();
	return 0;

    
    
    p = (void *)func;

    print_data(p);

    func();
/*
    for (i = 5; i < NUM; ++i) {
        ((unsigned long *)p)[i] = 0x00;        
    }
*/
    ((unsigned long *)p)[10] = 0xe3a01010;
    mb();
    print_data(p);
    
    func();
    
#if DEBUG == 1
    printk(__FUNCTION__);
#endif
	return 0;
}

void hijack_func_exit(void)
{
#if DEBUG == 1
    printk(__FUNCTION__);
#endif
}

module_init(hijack_func_init);
module_exit(hijack_func_exit);


MODULE_LICENSE("GPL");
MODULE_AUTHOR("tpl.a1406@gmail.com");

